﻿#include "stdlib.h"
#include "math.h"
#include "stdio.h"
#include "MatrixCompute.h"

/**
 * @brief damul 实矩阵相乘, 计算矩阵A（m*n）和B（n*k）的乘积，结果保存在C（m*k）中
 * @param a 长度为m*n的数组
 * @param b 长度为n*k的数组
 * @param m
 * @param n
 * @param k
 * @param c 长度为m*k的数组，存放结果
 */
void damul(const double matA[], const double matB[], int m, int n, int k, double matC[])
{
    int i, j, l, u;
    for(i = 0; i < m; i++)
    {
        for(j = 0; j < k; j++)
        {
            u = i * k + j;
            matC[u] = 0.0;
            for(l = 0; l < n; l++)
            {
                matC[u] += matA[i * n + l] * matB[l * k + j];
            }
        }
    }
    return;
}

/**
 * @brief ATdotB  计算矩阵A（m*n）的转置矩阵AT（n*m）和B（m*k）的乘积，结果保存在C（n*k）中
 * @param a  长度为m*n的数组
 * @param b  长度为m*k的数组
 * @param m
 * @param n
 * @param k
 * @param c  长度为n*k的数组，存放结果
 */
void ATdotB(const double matA[], const double matB[], int m, int n, int k, double matC[])
{
    int i, j, l, u;
    for(i = 0; i < n; i++)
    {
        for(j = 0; j < k; j++)
        {
            u = i * k + j;
            matC[u] = 0.0;
            for(l = 0; l < m; l++)
            {
                matC[u] += matA[l * n + i] * matB[l * k + j];
            }
        }
    }
    return;
}


/**
 * @brief AdotBT  计算矩阵A（m*n）和B（k*n）的转置矩阵BT（n*k）的乘积，结果保存在C（m*k）中
 * @param a  长度为m*n的数组
 * @param b  长度为k*n的数组
 * @param m
 * @param n
 * @param k
 * @param c  长度为m*k的数组，存放结果
 */
void AdotBT(const double matA[], const double matB[], int m, int n, int k, double matC[])
{
    int i, j, l, u;
    for(i = 0; i < m; i++)
    {
        for(j = 0; j < k; j++)
        {
            u = i * k + j;
            matC[u] = 0.0;
            for(l = 0; l < n; l++)
            {
                matC[u] += matA[i * n + l] * matB[j * n + l];
            }
        }
    }
    return;
}

/**
 * @brief dcinv   实矩阵求逆, 全选主元高斯-约当法
 * @param matA  长度为n*n的数组, n*n矩阵
 * @param n  矩阵的维数
 * @param  permRow 辅助数据，长度为 n, 记录行交换结果
 * @param  permCol 辅助数据，长度为 n，记录列交换结果
 * @return
 */
int dcinv(double matA[], int n, int permRow[], int permCol[])
{
    int i, j, k, l, u, v;
    double d, p;
    for(k = 0; k <= n - 1; k++)
    {
        d = 0.0;
        for(i = k; i <= n - 1; i++)
        {
            for(j = k; j <= n - 1; j++)
            {
                l = i * n + j;
                p = fabs(matA[l]);
                if(p > d)
                {
                    d = p;
                    permRow[k] = i;
                    permCol[k] = j;
                }
            }
        }
        if(d + 1.0 == 1.0)
        {
            return(0);
        }
        if(permRow[k] != k)
        {
            for(j = 0; j <= n - 1; j++)
            {
                u = k * n + j;
                v = permRow[k] * n + j;
                p = matA[u];
                matA[u] = matA[v];
                matA[v] = p;
            }
        }
        if(permCol[k] != k)
        {
            for(i = 0; i <= n - 1; i++)
            {
                u = i * n + k;
                v = i * n + permCol[k];
                p = matA[u];
                matA[u] = matA[v];
                matA[v] = p;
            }
        }
        l = k * n + k;
        matA[l] = 1.0 / matA[l];
        for(j = 0; j <= n - 1; j++)
        {
            if(j != k)
            {
                u = k * n + j;
                matA[u] = matA[u] * matA[l];
            }
        }
        for(i = 0; i <= n - 1; i++)
        {
            if(i != k)
            {
                for(j = 0; j <= n - 1; j++)
                {
                    if(j != k)
                    {
                        u = i * n + j;
                        matA[u] = matA[u] - matA[i * n + k] * matA[k * n + j];
                    }
                }
            }
        }
        for(i = 0; i <= n - 1; i++)
        {
            if(i != k)
            {
                u = i * n + k;
                matA[u] = -matA[u] * matA[l];
            }
        }
    }
    for(k = n - 1; k >= 0; k--)
    {
        if(permCol[k] != k)
        {
            for(j = 0; j <= n - 1; j++)
            {
                u = k * n + j;
                v = permCol[k] * n + j;
                p = matA[u];
                matA[u] = matA[v];
                matA[v] = p;
            }
        }
        if(permRow[k] != k)
        {
            for(i = 0; i <= n - 1; i++)
            {
                u = i * n + k;
                v = i * n + permRow[k];
                p = matA[u];
                matA[u] = matA[v];
                matA[v] = p;
            }
        }
    }   
    return(1);
}


/**
 * @brief desgj   对称正定矩阵求逆
 * @param a  长度为n*n的数组, n*n矩阵
 * @param n  矩阵的维数
 * @param b  辅助数组，长度为 n， 存放一些临时计算结果
 * @return  > 0 表示成功， < 0 说明出现问题
 */
int desgj(double matA[], int n, double b[])
{
    int i, j, k, m;
    double w, g;
    for(k = 0; k <= n - 1; k++)
    {
        w = matA[0];
        if(fabs(w) + 1.0 == 1.0)
        {
            return(-2);
        }
        m = n - k - 1;
        for(i = 1; i <= n - 1; i++)
        {
            g = matA[i * n];
            b[i] = g / w;
            if(i <= m)
            {
                b[i] = -b[i];
            }
            for(j = 1; j <= i; j++)
            {
                matA[(i - 1)*n + j - 1] = matA[i * n + j] + g * b[j];
            }
        }
        matA[n * n - 1] = 1.0 / w;
        for(i = 1; i <= n - 1; i++)
        {
            matA[(n - 1)*n + i - 1] = b[i];
        }
    }
    for(i = 0; i <= n - 2; i++)
    {
        for(j = i + 1; j <= n - 1; j++)
        {
            matA[i * n + j] = matA[j * n + i];
        }
    }
    return(2);
}

/**
 * @brief dftrn  托伯利兹(Toeplitz)矩阵求逆的特兰持(Trench)方法
 * @param t      长度为n的数组,存放n阶T型矩阵中的上三角元素t0,t1,t2...tn-1
 * @param tt     长度为n的数组,从tt[1]开始依次存放tt[1]...tt[n-1]
 * @param n      矩阵的阶数
 * @param b      长度为n*n的数组，返回时存放逆矩阵
 * @return
 */
int dftrn(double t[], double tt[], int n, double b[])
{
    int i, j, k;
    double a, s, *c, *r, *p;
    c = new double(n);// malloc(n * sizeof(double));
    r = new double(n);//malloc(n * sizeof(double));
    p = new double(n);//malloc(n * sizeof(double));
    if(fabs(t[0]) + 1.0 == 1.0)
    {
        delete[] c;// free(c);
        delete[] r;//free(r);
        delete[] p;//free(p);
        return(-1);
    }
    a = t[0];
    c[0] = tt[1] / t[0];
    r[0] = t[1] / t[0];
    for(k = 0; k <= n - 3; k++)
    {
        s = 0.0;
        for(j = 1; j <= k + 1; j++)
        {
            s = s + c[k + 1 - j] * tt[j];
        }
        s = (s - tt[k + 2]) / a;
        for(i = 0; i <= k; i++)
        {
            p[i] = c[i] + s * r[k - i];
        }
        c[k + 1] = -s;
        s = 0.0;
        for(j = 1; j <= k + 1; j++)
        {
            s = s + r[k + 1 - j] * t[j];
        }
        s = (s - t[k + 2]) / a;
        for(i = 0; i <= k; i++)
        {
            r[i] = r[i] + s * c[k - i];
            c[k - i] = p[k - i];
        }
        r[k + 1] = -s;
        a = 0.0;
        for(j = 1; j <= k + 2; j++)
        {
            a = a + t[j] * c[j - 1];
        }
        a = t[0] - a;
        if(fabs(a) + 1.0 == 1.0)
        {
            delete[] c;// free(c);
            delete[] r;//free(r);
            delete[] p;//free(p);
            return(-1);
        }
    }
    b[0] = 1.0 / a;
    for(i = 0; i <= n - 2; i++)
    {
        k = i + 1;
        j = (i + 1) * n;
        b[k] = -r[i] / a;
        b[j] = -c[i] / a;
    }
    for(i = 0; i <= n - 1; i++)
    {
        for(j = 0; j <= n - 2; j++)
        {
            k = (i + 1) * n + j + 1;
            b[k] = b[i * n + j] - c[i] * b[j + 1];
            b[k] = b[k] + c[n - j - 2] * b[n - i - 1];
        }
    }
    delete[] c;// free(c);
    delete[] r;//free(r);
    delete[] p;//free(p);
    return(1);
}


/**
 * @brief dhdet  求矩阵的行列式值, 采用全选主元高斯消去法
 * @param matA      长度为n*n的数组, matA 的内容会被破坏
 * @param n      矩阵的阶数
 * @return
 */
double dhdet(double matA[], int n)
{
    int i, j, k, is = 0, js = 0, l, u, v;
    double f, det, q, d;
    f = 1.0;
    det = 1.0;
    for(k = 0; k <= n - 2; k++)
    {
        q = 0.0;
        for(i = k; i <= n - 1; i++)
            for(j = k; j <= n - 1; j++)
            {
                l = i * n + j;
                d = fabs(matA[l]);
                if(d > q)
                {
                    q = d;
                    is = i;
                    js = j;
                }
            }
        if(q + 1.0 == 1.0)
        {
            det = 0.0;
            return(det);
        }
        if(is != k)
        {
            f = -f;
            for(j = k; j <= n - 1; j++)
            {
                u = k * n + j;
                v = is * n + j;
                d = matA[u];
                matA[u] = matA[v];
                matA[v] = d;
            }
        }
        if(js != k)
        {
            f = -f;
            for(i = k; i <= n - 1; i++)
            {
                u = i * n + js;
                v = i * n + k;
                d = matA[u];
                matA[u] = matA[v];
                matA[v] = d;
            }
        }
        l = k * n + k;
        det = det * matA[l];
        for(i = k + 1; i <= n - 1; i++)
        {
            d = matA[i * n + k] / matA[l];
            for(j = k + 1; j <= n - 1; j++)
            {
                u = i * n + j;
                matA[u] = matA[u] - d * matA[k * n + j];
            }
        }
    }
    det = f * det * matA[n * n - 1];
    return(det);
}



/**
 * @brief dicll   对称正定矩阵的乔里斯基(Cholesky)分解与行列式求值
 * @param matA    长度为n*n的数组，存放正定矩阵，返回时下三角部分存放分解后的下三角矩阵L,其余元素为0
 * @param n   正定矩阵的阶数
 * @param det  指向双精度实型变量的指针，返回时该指针指向的变量存放行列式的值
 * @return   返回值小于0表示程序工作失败;  返回值大于0表示正常返回
 */
int dicll(double matA[], int n, double  * det)
{
    int i, j, k, u, l;
    double d;
    if((matA[0] + 1.0 == 1.0) || (matA[0] < 0.0))
    {
        return(-2);
    }
    matA[0] = sqrt(matA[0]);
    d = matA[0];
    for(i = 1; i <= n - 1; i++)
    {
        u = i * n;
        matA[u] = matA[u] / matA[0];
    }
    for(j = 1; j <= n - 1; j++)
    {
        l = j * n + j;
        for(k = 0; k <= j - 1; k++)
        {
            u = j * n + k;
            matA[l] = matA[l] - matA[u] * matA[u];
        }
        if((matA[l] + 1.0 == 1.0) || (matA[l] < 0.0))
        {
            return(-2);
        }
        matA[l] = sqrt(matA[l]);
        d = d * matA[l];
        for(i = j + 1; i <= n - 1; i++)
        {
            u = i * n + j;
            for(k = 0; k <= j - 1; k++)
            {
                matA[u] = matA[u] - matA[i * n + k] * matA[j * n + k];
            }
            matA[u] = matA[u] / matA[l];
        }
    }
    *det = d * d;
    for(i = 0; i <= n - 2; i++)
    {
        for(j = i + 1; j <= n - 1; j++)
        {
            matA[i * n + j] = 0.0;
        }
    }
    return(2);
}

/**
 * @brief djlu  矩阵的三角分解(LU), 其中下三角阵L的主对角元素为1。
 * @param matA  长度为N*N的矩阵，返回时为L+U-I
 * @param n     矩阵的阶数
 * @param matL  返回下三角矩阵
 * @param matU  返回上三角矩阵
 * @return
 */
int djlu(double matA[], int n, double matL[], double matU[])
{
    int i, j, k, w, v, ll;
    for(k = 0; k <= n - 2; k++)
    {
        ll = k * n + k;
        if(fabs(matA[ll]) + 1.0 == 1.0)
        {
            return(0);
        }
        for(i = k + 1; i <= n - 1; i++)
        {
            w = i * n + k;
            matA[w] = matA[w] / matA[ll];
        }
        for(i = k + 1; i <= n - 1; i++)
        {
            w = i * n + k;
            for(j = k + 1; j <= n - 1; j++)
            {
                v = i * n + j;
                matA[v] = matA[v] - matA[w] * matA[k * n + j];
            }
        }
    }
    for(i = 0; i <= n - 1; i++)
    {
        for(j = 0; j < i; j++)
        {
            w = i * n + j;
            matL[w] = matA[w];
            matU[w] = 0.0;
        }
        w = i * n + i;
        matL[w] = 1.0;
        matU[w] = matA[w];
        for(j = i + 1; j <= n - 1; j++)
        {
            w = i * n + j;
            matL[w] = 0.0;
            matU[w] = matA[w];
        }
    }
    return(1);
}

/**
 * @brief dkqr  实数矩阵的QR分解法, 用Householder变换对一般m*n阶的实数矩阵进行QR分解
 * @param matA  长度为m*n的一维数组,返回时其左上三角部分存放上三角矩阵R
 * @param m     矩阵的行数
 * @param n     矩阵的列数
 * @param matQ  长度为m*m的矩阵，返回时存放正交矩阵Q
 * @return
 */
int dkqr(double matA[], int m, int n, double matQ[])
{
    int i, j, k, l, nn, p, jj;
    double u, alpha, w, t;
    if(m < n)
    {
        return(0);
    }
    for(i = 0; i <= m - 1; i++)
    {
        for(j = 0; j <= m - 1; j++)
        {
            l = i * m + j;
            matQ[l] = 0.0;
            if(i == j)
            {
                matQ[l] = 1.0;
            }
        }
    }
    nn = n;
    if(m == n)
    {
        nn = m - 1;
    }
    for(k = 0; k <= nn - 1; k++)
    {
        u = 0.0;
        l = k * n + k;
        for(i = k; i <= m - 1; i++)
        {
            w = fabs(matA[i * n + k]);
            if(w > u)
            {
                u = w;
            }
        }
        alpha = 0.0;
        for(i = k; i <= m - 1; i++)
        {
            t = matA[i * n + k] / u;
            alpha = alpha + t * t;
        }
        if(matA[l] > 0.0)
        {
            u = -u;
        }
        alpha = u * sqrt(alpha);
        if(fabs(alpha) + 1.0 == 1.0)
        {
            return(0);
        }
        u = sqrt(2.0 * alpha * (alpha - matA[l]));
        if((u + 1.0) != 1.0)
        {
            matA[l] = (matA[l] - alpha) / u;
            for(i = k + 1; i <= m - 1; i++)
            {
                p = i * n + k;
                matA[p] = matA[p] / u;
            }
            for(j = 0; j <= m - 1; j++)
            {
                t = 0.0;
                for(jj = k; jj <= m - 1; jj++)
                {
                    t = t + matA[jj * n + k] * matQ[jj * m + j];
                }
                for(i = k; i <= m - 1; i++)
                {
                    p = i * m + j;
                    matQ[p] = matQ[p] - 2.0 * t * matA[i * n + k];
                }
            }
            for(j = k + 1; j <= n - 1; j++)
            {
                t = 0.0;
                for(jj = k; jj <= m - 1; jj++)
                {
                    t = t + matA[jj * n + k] * matA[jj * n + j];
                }
                for(i = k; i <= m - 1; i++)
                {
                    p = i * n + j;
                    matA[p] = matA[p] - 2.0 * t * matA[i * n + k];
                }
            }
            matA[l] = alpha;
            for(i = k + 1; i <= m - 1; i++)
            {
                matA[i * n + k] = 0.0;
            }
        }
    }
    for(i = 0; i <= m - 2; i++)
    {
        for(j = i + 1; j <= m - 1; j++)
        {
            p = i * m + j;
            l = j * m + i;
            t = matQ[p];
            matQ[p] = matQ[l];
            matQ[l] = t;
        }
    }
    return(1);
}

/**
 * @brief dginv  奇异值分解法求广义逆, 调用函数：dluav()
 * @param matA   长度为m*n的数组，返回时其对角线依次给出奇异值，其余元素为0
 * @param m   矩阵的行数
 * @param n   矩阵的列数
 * @param aa  长度为n*m的数组，返回式存放A的广义逆
 * @param eps  精度要求
 * @param matU  长度为m*m的数组，返回时存放奇异值分解的左奇异量U
 * @param matV  长度为n*n的数组，返回时存放奇异值分解的左奇异量V
 * @return  本函数返回值小于0表示在奇异值分解过程, 中迭代值超过了60次还未满足精度要求. 返回值大于0表示正常返回。
 */
int dginv(double matA[], int m, int n, double aa[], double eps, double matU[], double matV[])
{
    int i, j, k, l, t, p, q, f;
    i = dluav(matA, m, n, matU, matV, eps);
    if(i < 0)
    {
        return(-1);
    }
    j = n;
    if(m < n)
    {
        j = m;
    }
    j = j - 1;
    k = 0;
    while((k <= j) && (matA[k * n + k] != 0.0))
    {
        k = k + 1;
    }
    k = k - 1;
    for(i = 0; i <= n - 1; i++)
    {
        for(j = 0; j <= m - 1; j++)
        {
            t = i * m + j;
            aa[t] = 0.0;
            for(l = 0; l <= k; l++)
            {
                f = l * n + i;
                p = j * m + l;
                q = l * n + l;
                aa[t] = aa[t] + matV[f] * matU[p] / matA[q];
            }
        }
    }
    return(1);
}

static void ppp(double a[], double e[], double s[], double v[], int m, int n);
static void sss(double fg[2], double cs[2]);

/**
 * @brief dluav  实数矩阵的奇异值分解, 用Householder变换及变形QR算法. 调用函数：dluav(),ppp(),sss()
 * @param matA   长度为m*n的数组，返回时其对角线依次给出奇异值，其余元素为0
 * @param m      矩阵的行数
 * @param n      矩阵的列数
 * @param matU   长度为m*m的数组，返回时存放奇异值分解的左奇异量U
 * @param matV   长度为n*n的数组，返回时存放奇异值分解的左奇异量V
 * @param eps  精度要求
 * @return
 */
int dluav(double matA[], int m, int n, double matU[], double matV[], double eps)
{
    int i, j, k, l, it, ll, kk, ix, iy, mm, nn, iz, m1, ks;
    double d, dd, t, sm, sm1, em1, sk, ek, b, c, shh, fg[2], cs[2];
    double *s, *e, *w;
    int ka = (m > n) ? m + 1 : n + 1;
    s = new double[static_cast<unsigned int >(ka)]; // malloc(ka * sizeof(double));
    e = new double[static_cast<unsigned int >(ka)]; // malloc(ka * sizeof(double));
    w = new double[static_cast<unsigned int >(ka)]; // malloc(ka * sizeof(double));
    it = 60;
    k = n;
    if(m - 1 < n)
    {
        k = m - 1;
    }
    l = m;
    if(n - 2 < m)
    {
        l = n - 2;
    }
    if(l < 0)
    {
        l = 0;
    }
    ll = k;
    if(l > k)
    {
        ll = l;
    }
    if(ll >= 1)
    {
        for(kk = 1; kk <= ll; kk++)
        {
            if(kk <= k)
            {
                d = 0.0;
                for(i = kk; i <= m; i++)
                {
                    ix = (i - 1) * n + kk - 1;
                    d = d + matA[ix] * matA[ix];
                }
                s[kk - 1] = sqrt(d);
                if(s[kk - 1] != 0.0)
                {
                    ix = (kk - 1) * n + kk - 1;
                    if(matA[ix] != 0.0)
                    {
                        s[kk - 1] = fabs(s[kk - 1]);
                        if(matA[ix] < 0.0)
                        {
                            s[kk - 1] = -s[kk - 1];
                        }
                    }
                    for(i = kk; i <= m; i++)
                    {
                        iy = (i - 1) * n + kk - 1;
                        matA[iy] = matA[iy] / s[kk - 1];
                    }
                    matA[ix] = 1.0 + matA[ix];
                }
                s[kk - 1] = -s[kk - 1];
            }
            if(n >= kk + 1)
            {
                for(j = kk + 1; j <= n; j++)
                {
                    if((kk <= k) && (s[kk - 1] != 0.0))
                    {
                        d = 0.0;
                        for(i = kk; i <= m; i++)
                        {
                            ix = (i - 1) * n + kk - 1;
                            iy = (i - 1) * n + j - 1;
                            d = d + matA[ix] * matA[iy];
                        }
                        d = -d / matA[(kk - 1) * n + kk - 1];
                        for(i = kk; i <= m; i++)
                        {
                            ix = (i - 1) * n + j - 1;
                            iy = (i - 1) * n + kk - 1;
                            matA[ix] = matA[ix] + d * matA[iy];
                        }
                    }
                    e[j - 1] = matA[(kk - 1) * n + j - 1];
                }
            }
            if(kk <= k)
            {
                for(i = kk; i <= m; i++)
                {
                    ix = (i - 1) * m + kk - 1;
                    iy = (i - 1) * n + kk - 1;
                    matU[ix] = matA[iy];
                }
            }
            if(kk <= l)
            {
                d = 0.0;
                for(i = kk + 1; i <= n; i++)
                {
                    d = d + e[i - 1] * e[i - 1];
                }
                e[kk - 1] = sqrt(d);
                if(e[kk - 1] != 0.0)
                {
                    if(e[kk] != 0.0)
                    {
                        e[kk - 1] = fabs(e[kk - 1]);
                        if(e[kk] < 0.0)
                        {
                            e[kk - 1] = -e[kk - 1];
                        }
                    }
                    for(i = kk + 1; i <= n; i++)
                    {
                        e[i - 1] = e[i - 1] / e[kk - 1];
                    }
                    e[kk] = 1.0 + e[kk];
                }
                e[kk - 1] = -e[kk - 1];
                if((kk + 1 <= m) && (e[kk - 1] != 0.0))
                {
                    for(i = kk + 1; i <= m; i++)
                    {
                        w[i - 1] = 0.0;
                    }
                    for(j = kk + 1; j <= n; j++)
                    {
                        for(i = kk + 1; i <= m; i++)
                        {
                            w[i - 1] = w[i - 1] + e[j - 1] * matA[(i - 1) * n + j - 1];
                        }
                    }
                    for(j = kk + 1; j <= n; j++)
                    {
                        for(i = kk + 1; i <= m; i++)
                        {
                            ix = (i - 1) * n + j - 1;
                            matA[ix] = matA[ix] - w[i - 1] * e[j - 1] / e[kk];
                        }
                    }
                }
                for(i = kk + 1; i <= n; i++)
                {
                    matV[(i - 1)*n + kk - 1] = e[i - 1];
                }
            }
        }
    }
    mm = n;
    if(m + 1 < n)
    {
        mm = m + 1;
    }
    if(k < n)
    {
        s[k] = matA[k * n + k];
    }
    if(m < mm)
    {
        s[mm - 1] = 0.0;
    }
    if(l + 1 < mm)
    {
        e[l] = matA[l * n + mm - 1];
    }
    e[mm - 1] = 0.0;
    nn = m;
    if(m > n)
    {
        nn = n;
    }
    if(nn >= k + 1)
    {
        for(j = k + 1; j <= nn; j++)
        {
            for(i = 1; i <= m; i++)
            {
                matU[(i - 1)*m + j - 1] = 0.0;
            }
            matU[(j - 1)*m + j - 1] = 1.0;
        }
    }
    if(k >= 1)
    {
        for(ll = 1; ll <= k; ll++)
        {
            kk = k - ll + 1;
            iz = (kk - 1) * m + kk - 1;
            if(s[kk - 1] != 0.0)
            {
                if(nn >= kk + 1)
                {
                    for(j = kk + 1; j <= nn; j++)
                    {
                        d = 0.0;
                        for(i = kk; i <= m; i++)
                        {
                            ix = (i - 1) * m + kk - 1;
                            iy = (i - 1) * m + j - 1;
                            d = d + matU[ix] * matU[iy] / matU[iz];
                        }
                        d = -d;
                        for(i = kk; i <= m; i++)
                        {
                            ix = (i - 1) * m + j - 1;
                            iy = (i - 1) * m + kk - 1;
                            matU[ix] = matU[ix] + d * matU[iy];
                        }
                    }
                }
                for(i = kk; i <= m; i++)
                {
                    ix = (i - 1) * m + kk - 1;
                    matU[ix] = -matU[ix];
                }
                matU[iz] = 1.0 + matU[iz];
                if(kk - 1 >= 1)
                {
                    for(i = 1; i <= kk - 1; i++)
                    {
                        matU[(i - 1)*m + kk - 1] = 0.0;
                    }
                }
            }
            else
            {
                for(i = 1; i <= m; i++)
                {
                    matU[(i - 1)*m + kk - 1] = 0.0;
                }
                matU[(kk - 1)*m + kk - 1] = 1.0;
            }
        }
    }
    for(ll = 1; ll <= n; ll++)
    {
        kk = n - ll + 1;
        iz = kk * n + kk - 1;
        if((kk <= l) && (e[kk - 1] != 0.0))
        {
            for(j = kk + 1; j <= n; j++)
            {
                d = 0.0;
                for(i = kk + 1; i <= n; i++)
                {
                    ix = (i - 1) * n + kk - 1;
                    iy = (i - 1) * n + j - 1;
                    d = d + matV[ix] * matV[iy] / matV[iz];
                }
                d = -d;
                for(i = kk + 1; i <= n; i++)
                {
                    ix = (i - 1) * n + j - 1;
                    iy = (i - 1) * n + kk - 1;
                    matV[ix] = matV[ix] + d * matV[iy];
                }
            }
        }
        for(i = 1; i <= n; i++)
        {
            matV[(i - 1)*n + kk - 1] = 0.0;
        }
        matV[iz - n] = 1.0;
    }
    for(i = 1; i <= m; i++)
    {
        for(j = 1; j <= n; j++)
        {
            matA[(i - 1)*n + j - 1] = 0.0;
        }
    }
    m1 = mm;
    it = 60;
    while(1 == 1)
    {
        if(mm == 0)
        {
            ppp(matA, e, s, matV, m, n);
            delete[] s;
            delete[] e;
            delete[] w;
//            free(s);
//            free(e);
//            free(w);
            return(1);
        }
        if(it == 0)
        {
            ppp(matA, e, s, matV, m, n);
            delete[] s;
            delete[] e;
            delete[] w;
//            free(s);
//            free(e);
//            free(w);
            return(-1);
        }
        kk = mm - 1;
        while((kk != 0) && (fabs(e[kk - 1]) != 0.0))
        {
            d = fabs(s[kk - 1]) + fabs(s[kk]);
            dd = fabs(e[kk - 1]);
            if(dd > eps * d)
            {
                kk = kk - 1;
            }
            else
            {
                e[kk - 1] = 0.0;
            }
        }
        if(kk == mm - 1)
        {
            kk = kk + 1;
            if(s[kk - 1] < 0.0)
            {
                s[kk - 1] = -s[kk - 1];
                for(i = 1; i <= n; i++)
                {
                    ix = (i - 1) * n + kk - 1;
                    matV[ix] = -matV[ix];
                }
            }
            while((kk != m1) && (s[kk - 1] < s[kk]))
            {
                d = s[kk - 1];
                s[kk - 1] = s[kk];
                s[kk] = d;
                if(kk < n)
                {
                    for(i = 1; i <= n; i++)
                    {
                        ix = (i - 1) * n + kk - 1;
                        iy = (i - 1) * n + kk;
                        d = matV[ix];
                        matV[ix] = matV[iy];
                        matV[iy] = d;
                    }
                }
                if(kk < m)
                {
                    for(i = 1; i <= m; i++)
                    {
                        ix = (i - 1) * m + kk - 1;
                        iy = (i - 1) * m + kk;
                        d = matU[ix];
                        matU[ix] = matU[iy];
                        matU[iy] = d;
                    }
                }
                kk = kk + 1;
            }
            it = 60;
            mm = mm - 1;
        }
        else
        {
            ks = mm;
            while((ks > kk) && (fabs(s[ks - 1]) != 0.0))
            {
                d = 0.0;
                if(ks != mm)
                {
                    d = d + fabs(e[ks - 1]);
                }
                if(ks != kk + 1)
                {
                    d = d + fabs(e[ks - 2]);
                }
                dd = fabs(s[ks - 1]);
                if(dd > eps * d)
                {
                    ks = ks - 1;
                }
                else
                {
                    s[ks - 1] = 0.0;
                }
            }
            if(ks == kk)
            {
                kk = kk + 1;
                d = fabs(s[mm - 1]);
                t = fabs(s[mm - 2]);
                if(t > d)
                {
                    d = t;
                }
                t = fabs(e[mm - 2]);
                if(t > d)
                {
                    d = t;
                }
                t = fabs(s[kk - 1]);
                if(t > d)
                {
                    d = t;
                }
                t = fabs(e[kk - 1]);
                if(t > d)
                {
                    d = t;
                }
                sm = s[mm - 1] / d;
                sm1 = s[mm - 2] / d;
                em1 = e[mm - 2] / d;
                sk = s[kk - 1] / d;
                ek = e[kk - 1] / d;
                b = ((sm1 + sm) * (sm1 - sm) + em1 * em1) / 2.0;
                c = sm * em1;
                c = c * c;
                shh = 0.0;
                if((b != 0.0) || (c != 0.0))
                {
                    shh = sqrt(b * b + c);
                    if(b < 0.0)
                    {
                        shh = -shh;
                    }
                    shh = c / (b + shh);
                }
                fg[0] = (sk + sm) * (sk - sm) - shh;
                fg[1] = sk * ek;
                for(i = kk; i <= mm - 1; i++)
                {
                    sss(fg, cs);
                    if(i != kk)
                    {
                        e[i - 2] = fg[0];
                    }
                    fg[0] = cs[0] * s[i - 1] + cs[1] * e[i - 1];
                    e[i - 1] = cs[0] * e[i - 1] - cs[1] * s[i - 1];
                    fg[1] = cs[1] * s[i];
                    s[i] = cs[0] * s[i];
                    if((cs[0] != 1.0) || (cs[1] != 0.0))
                    {
                        for(j = 1; j <= n; j++)
                        {
                            ix = (j - 1) * n + i - 1;
                            iy = (j - 1) * n + i;
                            d = cs[0] * matV[ix] + cs[1] * matV[iy];
                            matV[iy] = -cs[1] * matV[ix] + cs[0] * matV[iy];
                            matV[ix] = d;
                        }
                    }
                    sss(fg, cs);
                    s[i - 1] = fg[0];
                    fg[0] = cs[0] * e[i - 1] + cs[1] * s[i];
                    s[i] = -cs[1] * e[i - 1] + cs[0] * s[i];
                    fg[1] = cs[1] * e[i];
                    e[i] = cs[0] * e[i];
                    if(i < m)
                    {
                        if((cs[0] != 1.0) || (cs[1] != 0.0))
                        {
                            for(j = 1; j <= m; j++)
                            {
                                ix = (j - 1) * m + i - 1;
                                iy = (j - 1) * m + i;
                                d = cs[0] * matU[ix] + cs[1] * matU[iy];
                                matU[iy] = -cs[1] * matU[ix] + cs[0] * matU[iy];
                                matU[ix] = d;
                            }
                        }
                    }
                }
                e[mm - 2] = fg[0];
                it = it - 1;
            }
            else
            {
                if(ks == mm)
                {
                    kk = kk + 1;
                    fg[1] = e[mm - 2];
                    e[mm - 2] = 0.0;
                    for(ll = kk; ll <= mm - 1; ll++)
                    {
                        i = mm + kk - ll - 1;
                        fg[0] = s[i - 1];
                        sss(fg, cs);
                        s[i - 1] = fg[0];
                        if(i != kk)
                        {
                            fg[1] = -cs[1] * e[i - 2];
                            e[i - 2] = cs[0] * e[i - 2];
                        }
                        if((cs[0] != 1.0) || (cs[1] != 0.0))
                        {
                            for(j = 1; j <= n; j++)
                            {
                                ix = (j - 1) * n + i - 1;
                                iy = (j - 1) * n + mm - 1;
                                d = cs[0] * matV[ix] + cs[1] * matV[iy];
                                matV[iy] = -cs[1] * matV[ix] + cs[0] * matV[iy];
                                matV[ix] = d;
                            }
                        }
                    }
                }
                else
                {
                    kk = ks + 1;
                    fg[1] = e[kk - 2];
                    e[kk - 2] = 0.0;
                    for(i = kk; i <= mm; i++)
                    {
                        fg[0] = s[i - 1];
                        sss(fg, cs);
                        s[i - 1] = fg[0];
                        fg[1] = -cs[1] * e[i - 1];
                        e[i - 1] = cs[0] * e[i - 1];
                        if((cs[0] != 1.0) || (cs[1] != 0.0))
                        {
                            for(j = 1; j <= m; j++)
                            {
                                ix = (j - 1) * m + i - 1;
                                iy = (j - 1) * m + kk - 2;
                                d = cs[0] * matU[ix] + cs[1] * matU[iy];
                                matU[iy] = -cs[1] * matU[ix] + cs[0] * matU[iy];
                                matU[ix] = d;
                            }
                        }
                    }
                }
            }
        }
    }
    return(1);
}

static void ppp(double a[], double e[], double s[], double v[], int m, int n)
{
    int i, j, p, q;
    double d;
    if(m >= n)
    {
        i = n;
    }
    else
    {
        i = m;
    }
    for(j = 1; j <= i - 1; j++)
    {
        a[(j - 1)*n + j - 1] = s[j - 1];
        a[(j - 1)*n + j] = e[j - 1];
    }
    a[(i - 1)*n + i - 1] = s[i - 1];
    if(m < n)
    {
        a[(i - 1)*n + i] = e[i - 1];
    }
    for(i = 1; i <= n - 1; i++)
    {
        for(j = i + 1; j <= n; j++)
        {
            p = (i - 1) * n + j - 1;
            q = (j - 1) * n + i - 1;
            d = v[p];
            v[p] = v[q];
            v[q] = d;
        }
    }
    return;
}

static void sss(double fg[2], double cs[2])
{
    double r, d;
    if((fabs(fg[0]) + fabs(fg[1])) == 0.0)
    {
        cs[0] = 1.0;
        cs[1] = 0.0;
        d = 0.0;
    }
    else
    {
        d = sqrt(fg[0] * fg[0] + fg[1] * fg[1]);
        if(fabs(fg[0]) > fabs(fg[1]))
        {
            d = fabs(d);
            if(fg[0] < 0.0)
            {
                d = -d;
            }
        }
        if(fabs(fg[1]) >= fabs(fg[0]))
        {
            d = fabs(d);
            if(fg[1] < 0.0)
            {
                d = -d;
            }
        }
        cs[0] = fg[0] / d;
        cs[1] = fg[1] / d;
    }
    r = 1.0;
    if(fabs(fg[0]) > fabs(fg[1]))
    {
        r = cs[1];
    }
    else
    {
        if(cs[0] != 0.0)
        {
            r = 1.0 / cs[0];
        }
        fg[0] = d;
        fg[1] = r;
        return;
    }
}

/**
 * @brief dbcmul  复数矩阵相乘,计算矩阵A（m*n）和B（n*k）的乘积，结果保存在C（m*k）中
 * @param matAR   长度为m*n的数组,存放A的实部
 * @param matAI   长度为m*n的数组,存放A的虚部
 * @param matBR   长度为n*k的数组,存放B的实部
 * @param matBI   长度为n*k的数组,存放B的虚部
 * @param m
 * @param n
 * @param k
 * @param matCR   长度为m*k的数组，存放结果C的实部
 * @param matCI   长度为m*k的数组，存放结果C的虚部
 */
void dbcmul(const double matAR[], const double matAI[], const double matBR[], const double matBI[], int m, int n, int k, double matCR[], double matCI[])
{
    int i, j, l, u, v, w;
    double p, q, s;
    for(i = 0; i <= m - 1; i++)
    {
        for(j = 0; j <= k - 1; j++)
        {
            u = i * k + j;
            matCR[u] = 0.0;
            matCI[u] = 0.0;
            for(l = 0; l <= n - 1; l++)
            {
                v = i * n + l;
                w = l * k + j;
                p = matAR[v] * matBR[w];
                q = matAI[v] * matBI[w];
                s = (matAR[v] + matAI[v]) * (matBR[w] + matBI[w]);
                matCR[u] = matCR[u] + p - q;
                matCI[u] = matCI[u] + s - p - q;
            }
        }
    }
    return;
}

/**
 * @brief ddcinv  复数矩阵求逆
 * @param matAR   长度为n*n的数组, n*n矩阵的实部
 * @param matAI   长度为n*n的数组, n*n矩阵的虚部
 * @param n       矩阵的维数
 * @return
 */
int ddcinv(double matAR[], double matAI[], int n)
{
    int *is, *js, i, j, k, l, u, v, w;
    double p, q, s, t, d, b;
    is = new int(n);// malloc(n * sizeof(int));
    js = new int(n);// malloc(n * sizeof(int));
    for(k = 0; k <= n - 1; k++)
    {
        d = 0.0;
        for(i = k; i <= n - 1; i++)
        {
            for(j = k; j <= n - 1; j++)
            {
                u = i * n + j;
                p = matAR[u] * matAR[u] + matAI[u] * matAI[u];
                if(p > d)
                {
                    d = p;
                    is[k] = i;
                    js[k] = j;
                }
            }
        }
        if(d + 1.0 == 1.0)
        {
            delete [] is;//free(is);
            delete [] js;//free(js);
            return(0);
        }
        if(is[k] != k)
        {
            for(j = 0; j <= n - 1; j++)
            {
                u = k * n + j;
                v = is[k] * n + j;
                t = matAR[u];
                matAR[u] = matAR[v];
                matAR[v] = t;
                t = matAI[u];
                matAI[u] = matAI[v];
                matAI[v] = t;
            }
        }
        if(js[k] != k)
        {
            for(i = 0; i <= n - 1; i++)
            {
                u = i * n + k;
                v = i * n + js[k];
                t = matAR[u];
                matAR[u] = matAR[v];
                matAR[v] = t;
                t = matAI[u];
                matAI[u] = matAI[v];
                matAI[v] = t;
            }
        }
        l = k * n + k;
        matAR[l] = matAR[l] / d;
        matAI[l] = -matAI[l] / d;
        for(j = 0; j <= n - 1; j++)
        {
            if(j != k)
            {
                u = k * n + j;
                p = matAR[u] * matAR[l];
                q = matAI[u] * matAI[l];
                s = (matAR[u] + matAI[u]) * (matAR[l] + matAI[l]);
                matAR[u] = p - q;
                matAI[u] = s - p - q;
            }
        }
        for(i = 0; i <= n - 1; i++)
        {
            if(i != k)
            {
                v = i * n + k;
                for(j = 0; j <= n - 1; j++)
                {
                    if(j != k)
                    {
                        u = k * n + j;
                        w = i * n + j;
                        p = matAR[u] * matAR[v];
                        q = matAI[u] * matAI[v];
                        s = (matAR[u] + matAI[u]) * (matAR[v] + matAI[v]);
                        t = p - q;
                        b = s - p - q;
                        matAR[w] = matAR[w] - t;
                        matAI[w] = matAI[w] - b;
                    }
                }
            }
        }
        for(i = 0; i <= n - 1; i++)
        {
            if(i != k)
            {
                u = i * n + k;
                p = matAR[u] * matAR[l];
                q = matAI[u] * matAI[l];
                s = (matAR[u] + matAI[u]) * (matAR[l] + matAI[l]);
                matAR[u] = q - p;
                matAI[u] = p + q - s;
            }
        }
    }
    for(k = n - 1; k >= 0; k--)
    {
        if(js[k] != k)
        {
            for(j = 0; j <= n - 1; j++)
            {
                u = k * n + j;
                v = js[k] * n + j;
                t = matAR[u];
                matAR[u] = matAR[v];
                matAR[v] = t;
                t = matAI[u];
                matAI[u] = matAI[v];
                matAI[v] = t;
            }
        }
        if(is[k] != k)
        {
            for(i = 0; i <= n - 1; i++)
            {
                u = i * n + k;
                v = i * n + is[k];
                t = matAR[u];
                matAR[u] = matAR[v];
                matAR[v] = t;
                t = matAI[u];
                matAI[u] = matAI[v];
                matAI[v] = t;
            }
        }
    }
    delete [] is;//free(is);
    delete [] js;//free(js);
    return(1);
}
